#include "cmd_claw.h"
#include "claw.h"
#include "dji2_0.h"
#include "claw_signal.h"
#include "claw_motor.h"

/*指令测试抓手开合*/
void Command_ClawCtr(int argc, char *argv[])
{
    ClawCtr(atoi(argv[1]), atoi(argv[2]), atoi(argv[3]));
    uprintf("claw state:%d %d %d\r\n", atoi(argv[1]), atoi(argv[2]), atoi(argv[3]));
}

/*指令测试大疆电机*/
void Command_PosCtr(int argc, char *argv[])
{
    dji_PosCtrl(ClawCAN, ClawBoardID, atoi(argv[1]), atoi(argv[2]));
    uprintf("Motor %d Move to %d\r\n", atoi(argv[1]), atoi(argv[2]));
}
void Command_MotorInit(int argc, char *argv[])
{
    dji_PosCfg(ClawCAN, ClawBoardID, atoi(argv[1]), atoi(argv[2]));
    uprintf("Init Motor %d, maxvel is %d\r\n", atoi(argv[1]), atoi(argv[2]));
}
void Command_MotorOff(int argc, char *argv[])
{
    dji_MotorOff(ClawCAN, ClawBoardID, atoi(argv[1]));
    uprintf("Motor %d off!\r\n", atoi(argv[1]));
}

/*测试抓手移动homing*/
void Command_TestHoming(int argc, char *argv[])
{
    dji_Homing(ClawCAN, ClawBoardID, atoi(argv[1]), atoi(argv[2]), atoi(argv[3]));
    uprintf("start homing, id is %d, vel is %d, cur is %d\r\n", atoi(argv[1]), atoi(argv[2]), atoi(argv[3]));
}

/*指令控制开始抓箭*/
void Command_StartTakeArrow(int argc, char *argv[])
{
    Signal_StartTakeArrow();
}

/*指令控制流程抓手左移*/
void Command_MoveClaw(int argc, char *argv[])
{
    Signal_MoveClaw();
}

/*指令控制流程抓手开合*/
void Command_OpenClaw(int argc, char *argv[])
{
    Signal_OpenClaw();
}

void Command_OpenClawMachine(int argc, char *argv[])
{
    Sign_OpenClawMachine();
}

void Command_CloseClawMachine(int argc, char *argv[])
{
    Sign_CloseClawMachine();
}


